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Abstract 

Any  air-vehicle  can  be  thought  of  as  evolving  on  Riemannian  manifold  with  the  total  kinetic 
energy  as  the  metric.  In  this  paper,  we  first  derive  first-order  necessary  conditions  for  a  Bolza- 
type  optimal  control  problem  on  a  Riemannian  manifold.  Then  we  apply  this  theory  to  a  rotating 
rigid  body,  by  obtaining  expressions  for  the  Riemannian  connection  and  curvature  tensor  via 
Cartan’s  formalism. 

The  upshot  of  this  work  is  the  derivation  of  first-order  necessary  conditions  for  the  trajectory¬ 
planning  problem  that  are  singularity-free,  and  considerably  simpler  that  those  obtained  by  using 
an  Euler  angles  representation. 

We  next  apply  a  new  numerical  solution  technique  called  the  “Modified  Simple  Shooting 

Method”,  to  the  resulting  two-point  boundary  value  problem.  We  demonstrate  that  by  using 

the  new  necessary  conditions  and  the  numerical  method,  one  can  solve  the  trajectory  planning 

problem  extremely  fast  and  on-line  implementations  become  feasible. 
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1  Introduction 


Any  air-vehicle  can  be  thought  of  as  evolving  on  Riemannian  manifold  with  the  total  kinetic  energy 
as  the  Riemannian  metric.  If  we  consider  the  aerodynamic  forces  and  moments  to  be  inputs,  then 
the  state  variables  are  (Q,w,b,v)  where  Q  is  the  orientation  of  the  vehicle  with  respect  to  a  fixed 
co-ordinate  axis;  u  is  the  body  angular  velocity;  b  is  the  position  of  the  center-of-mass  from  the 
origin  of  the  fixed  co-ordinate  axis;  and  v  is  the  velocity  of  the  vehicle  in  the  body  axes  co-ordinates. 
The  state  space  is  therefore,  TSE( 3)  the  tangent  bundle  of  the  group  SE(3).  The  on-line  trajectory 
planning  problem  (in  case  of  actuator  failures)  for  aircraft  is  thus  naturally  posed  on  a  Riemannian 
manifold  rather  than  a  Euclidean  space.  In  this  paper,  we  first  study  optimal  control  problems 
on  parallelizable  Riemannian  manifolds.  Then  we  consider  the  solution  of  the  resulting  two-point 
boundary  value  problems  with  a  new  numerical  technique  called  the  modified  simple  shooting 
method. 

In  section  2,  we  derive  first-order  necessary  conditions  for  the  extremals  of  a  Bolza-type  optimal 
control  problem  on  parallelizable  Riemannian  manifolds  using  calculus  of  variations.  Previous  work 
in  this  area  was  done  by  P.  Crouch,  M.  Camarinha  and  F.  Silva  Leite  in  a  series  of  papers  (1,  2,  3]. 
Their  work  and  ours  differ  in  the  nature  of  variations  considered,  and  therefore  the  necessary 
conditions  obtained  in  this  paper  are  different  from  those  of  P.  Crouch,  M.  Camarinha  and  F.  Silva 
Leite. 

In  [4],  Sussmann  tackled  the  problem  of  generalizing  the  minimum  principle  (or  equivalently 
the  maximum  principle  by  considering  the  negative  of  the  cost  function),  to  manifolds  (without 
any  affine-connection  structure),  by  developing  the  co-ordinate  free  maximum  principle.  However, 
when  this  principle  is  applied  to  an  air-vehicle  problem,  one  employs  local  co-ordinates  and  the 
equations  reduce  to  the  necessary  conditions  for  an  optimal  control  problem  in  co-ordinates.  In 
contrast,  we  make  use  of  the  Riemannian  connection  and  employ  frame  co-ordinates  (we  assume 
the  manifold  to  be  parallelizable),  that  yield  global  equations.  Thus  the  method  is  frame  dependent 
rather  than  invariant.  This  idea  can  also  be  seen  in  the  work  of  P.  Crouch,  M.  Camarinha  and  F. 
Silva  Leite  [1,  2,  3].  In  subsection  3.1,  we  show  that  one  can  obtain  the  Noakes,  Heinzinger  and 
Paden  formula  for  cubic  splines  on  Riemannian  manifolds  [5]  by  specializing  Theorem  2.3. 

Finally,  we  apply  the  theory  to  the  path-planning  problem  for  a  rotating  rigid  body  as  an 
optimal  control  problem  on  TSO(3).  We  obtain  the  first  order  necessary  conditions  for  a  rigid 
body  without  the  use  of  local  co-ordinates,  but  rather  frame  co-ordinates.  In  Subgectiofi  3.2,  we 
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present  a  numerical  experiment  where  these  equations  are  solved  using  a  modified  simple  shooting 
method. 

2  Optimal  Control  on  Riemannian  Manifolds 

In  this  section,  we  derive  necessary  conditions  for  Bolza-type  optimal  control  problems  on  Rieman¬ 
nian  manifolds.  Previous  work  in  this  area  was  done  by  P.  Crouch,  M.  Camarinha  and  F.  Silva 
Leite  in  a  series  of  papers  [1,  2,  3].  However,  our  work  differs  in  the  nature  of  variations  considered, 
and  in  this  respect  more  closely  resembles  the  work  of  L.  Noakes,  G.  Heinzinger  and  B.  Paden  [5]. 
More  precisely,  suppose  that  M  is  a  Riemannian  manifold  with  V  as  the  Levi-Civita  connection. 
Suppose  further  that  (g,  V)(t),  t  £  [to,^/]  denotes  the  optimal  solution  as  a  curve  in  TM.  Here, 
q(t)  £  M,  and  V{t)  £  T^M,  t  £  [t0l  t/]-  Now  consider  a  one-parameter  family  of  variations  given 
by  (g,  V)(£,a),  t  £  [t0,t/],  o  £  (-e,e),  e  >  0.  For  optimal  control  problems  with  both  initial  and 
final  states  (that  is,  (g,  V)(t0)  and  (g,V)(t/))  are  prescribed,  the  admissible  set  of  variations  for 
Crouch  et  al.  are  those  for  which  the  ordinary  derivatives  and  vanish  at  to  and  tf.  In  this 
paper  however,  we  consider  |£,  and  the  total  or  covariant  derivative  of  the  velocity  V  j*  V  to  vanish 
at  to  and  tf.  The  justification  for  this,  is  a  lemma  proved  by  Noakes,  Heinzinger  and  Paden  [5]. 
Therefore,  the  necessary  conditions  obtained  in  this  work  are  different  from  those  of  P.  Crouch,  M. 
Camarinha  and  F.  Silva  Leite. 

2.1  Parallelizable  manifolds  and  Cart  an  Formalism 

This  subsection  details  the  mathematical  background  in  differential  geometry  used  in  the  rest  of 
the  paper.  There  are  several  good  sources  for  the  material  such  as  Frankel  [6]  and  Petersen  [7]. 
First,  let  us  consider  the  concept  of  parallelizable  manifolds. 

Definition  2.1  (Parallelizable  Manifolds )  [8]  If  there  exists  a  set  of  n  smooth  vector  fields  on  a 
manifold  M  of  dimension  n  that  are  linearly  independent  at  each  point ,  then  the  manifold  M  is 
said  to  be  parallelizable. 

The  above  set  of  n  linearly  independent  vector  fields  is  referred  to  as  a  field  of  co-ordinate 
frames  or  briefly  as  a  frame .  It  is  well  known  that  the  condition  of  being  parallelizable  is  very 
special  for  a  manifold.  For  example,  among  the  spheres  Sn ,  n  —  1,  2,  *  •  • ,  only  S1,  S3  and  S 7  are 
parallelizable  [8].  However,  the  following  theorem  states  that  all  Lie  groups  are  parallelizable. 
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Theorem  2.1  [8]  Let  G  be  a  Lie  group  and  Q  the  tangent  space  at  the  identity  e.  Then  each 
Xe  £  Q  determines  uniquely  a  C°°  vector  field  X  on  G  which  is  invariant  under  left  translations. 
In  particular,  G  is  parallelizable. 

In  the  case  of  air  vehicles,  the  configuration  space  as  mentioned  earlier  is  the  Lie  Group  SE(3). 
However  the  manifold  on  which  the  air  vehicle  dynamics  can  be  described  is  TSE( 3)  the  tangent 
bundle  of  SE(S).  So  we  need  the  following  lemma: 

Lemma  2.1  Let  M  be  a  parallelizable  manifold.  Then  TM  is  parallelizable. 

Proof 

This  follows  from  the  observation  that  locally  TM  is  a  product  manifold  U  x  Rn  where  U  is  a 
co-ordinate  neighborhood  of  M.  Now  Rn  is  parallelizable  as  it  is  a  Lie  Group.  Let  {Xi,  •  •  • ,  Xn} 
be  a  frame  on  M.  Then  we  can  define  vector  fields  Fij  —  (X, ,  Ej ),  i,  j  =  1,  •  •  • ,  n  where  Ej  are  the 
co-ordinate  vector  fields  for  R",  that  form  a  frame  on  TM.  ■ 

Now,  let  M  be  a  manifold  with  a  symmetric,  positive  definite,  and  bilinear  form  defined  on 
TqM  where  q  £  M,  denoted  by  <•,■>,  .  Such  a  form  is  usually  referred  to  as  Riemannian  metric 
and  we  assume  it  to  be  smooth  for  each  q  £  M  and  C1  as  a  function  of  q.  A  Riemannian  metric 
defines  an  linear  isomorphism  S  :  TqM  —*T*M  where  q  £  M,  by: 

(E(u))  (w):=  <  v,w>q\  v,w  £  TqM.  (1) 

As  S  is  an  isomorphism,  we  also  have  the  inverse  map  E-1  :T*M  — »  TqM  where  q  £  M. 

Let  't'(M)  denote  the  set  of  all  C°°  vector  fields  on  M.  If  X  €  T(M),  then  the  directional 
derivative  of  a  C°°  function  /  on  M  in  the  direction  of  X  is  another  C°°  function  denoted  by 
X(f(q)).  If  X,Y  £  T(M),  then  one  can  define  another  vector  field  called  the  Lie  bracket  of  X  and 

Y  by: 

[X,Y](f):=X(Y(f))-Y(X(f)).  (2) 

If  X,Y,Z  €  (A/),  then  an  affine  connection  is  an  operator  V  :  T(M)  x  T  ( M )  — >  T  (M), 

satisfying: (a)  V_x(aT  +  bZ)  —  a V_v Y  +  bX xX\  a,b  £  R,  (b)  V (oX  +  bY)Z  —  aX x Z  +  6V y Z.  and 
(c)  Xx(fY)  =  X(f)Y  +  fXxY ;  /  e  C°°(M). 

It  is  well  known  that  on  a  Riemannian  manifold  M,  there  exists  a  unique,  affine  connection 

V  :  T(AL)  x  'I'(M)  — >  ’i(M)  on  M  that  is  (a)  symmetric  (also  called  torsion- free),  i.e.  XxY  — 
VYX  =  [A,  Y],  X,  Y  £  'L(M)  and  (b)  compatible  with  the  Riemannian  metric,  that  is ,  X<Y,Z> 
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=  <  V*y,Z  >  +  <  Y,  VxZ  >,  X,Y,Z  G  SP(M)  [9].  Such  a  connection  is  called  a  Levi-Civita 
connection  on  M. 

Assume  M  to  be  parallelizable  and  consider  a  frame  {Ei}  and  a  dual  co-frame  {0*},  i  =  1, 2,  •  •  •  n 
with  d^Ej)  =  5j.  Then  the  Levi-Civita  connection  is  defined  by  the  functions  w^-(g)  in  the  equation: 

V  BiEj  =  uJijEk,  1  <i,j<n.  (3) 

If  the  vector  field  X  G  $(M)  is  defined  by  X  =  x‘.Ei,  then  we  have: 

VxJSj  =  x^EtBj 

=  x'UijEk 

=  (oj^Ektee*)  (x%), 

where  <S)  is  the  mixed  tensor  product  of  a  vector  and  co- vector.  Thus  if  77,  £  G  T*M  and  v,  w  G  TqM, 
then  v  <g>  r]  acts  on  (£,  w)  by: 

(v®T)){Z,w)  =  £(v)ri(w). 

Similarly,  one  can  define  a  tensor  product  of  two  forms 

(r)®0{v,w)  =  v(v)£(w). 

This  tensor  product  is  not  a  two-form.  The  wedge  product  of  two  one-forms  77,  £  G  T*M  is  a 
two-form  defined  by: 

77A£  =  77<g>£-£<8>?7. 

If  we  denote  uk-  ujkj  9\  then  we  can  define  the  n  x  n  matrix  u  of  connection  one-forms  by: 

w  :=(^).  (4) 

We  can  then  write  compactly: 

VEj  —  Ek®  ukj  •  (5) 

Thus  by  computing  the  connection  matrix  u,  one  can  obtain  the  Levi-Civita  connection  on  M.  For 
a  co-ordinate  frame ,  that  is,  a  frame  that  results  from  a  local  co-ordinate  chart,  the  connection 
coefficients  are  symmetric  in  the  lower  indices:  u)kj  =  This  is  due  to  the  fact  that  a  co-ordinate 
frame  has  the  property  that  the  Lie- bracket  of  the  co-ordinate  vector  fields,  [Ei,  Ej]  =  0,  1  <  i,  j  < 
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Figure  1:  Use  of  the  connection  to  map  vectors  on  TTM  to  TM. 

n .  However,  we  shall  not  use  co-ordinate  frames  in  our  study  of  the  rigid  body  in  Section  3.  If 
Y  G  with  Y  =  y^Ej  in  frame  co-ordinates  then: 

=  W)£j  +  x^oj^Ek 
=  xiEi(jfi)Ej+cjkj(X)y^Ek 
=  xiEi(yf)Ej  +  (u;(X))(X) 

One  can  use  a  connection  structure  to  map  a  vector  field  on  TTM  to  one  on  TM.  To  be 
precise,  let  km  :  TM  -*  M;  tt (q,V)  =  q  be  the  trivial  projection  map.  This  map  is  a  smooth 
map  in  the  topologies  of  TM  and  M.  Similarly,  we  have  the  trivial  projection,  7 xtm  ■  TTM  — > 
TM;  ■K((q,v),(w,r))  =  (q,  v).  On  the  other  hand,  we  have  differential  of  projection  map  on  M 
given  by,  dnM  ■  TTM  — >  TM;  dn{(q,v ),  (w,r))  =  (q,w).  The  kernel  of  d-K  at  every  q  e  M  defines 
the  so-called  vertical  subspace  of  T^TM.  In  co-ordinates,  this  subspace  consists  of  elements  of 
the  type  ((q,v),(0,r)).  A  horizontal  subspace  of  T^jTM  is  obtained  using  the  connection  V. 
Consider  the  vector  bundle  morphism  in  Figure  1.  The  kernel  of  the  map  A"  is  a  horizontal  subspace 
of  T(?|t,)TM.  In  co-ordinates,  this  subspace  consists  of  elements  of  the  type  ((q,  v),  (w,  —u(v,w))). 
These  two  subspaces  lead  to  a  splitting  of  T^V)TM  [10].  We  do  not  need  this  structure  in  this 
paper,  except  to  note  that  this  construction  yields  us  a  way  of  mapping  a  vector  field  on  TTM 
to  one  on  TM  via  the  connection  map  K.  We  use  this  mapping  to  define  a  second-order  control 
system  on  M  in  the  next  subsection. 

The  following  propositions  due  to  E.  Cartan  formalizes  the  computations: 

Proposition  2.1  [7]  Define  dd  =  (dO1,  ■  •  • ,  ddn).  Then  there  is  a  unique  matrix  of  1-forms  u  = 
(uilj  ^  such  that: 

dO  =  —w  A  6 
d6i  =  -u)  A0j 
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(6) 

(7) 


and 


(8) 


These  are  known  as  the  first  structural  equations.  The  first  equation  encodes  the  fact  that  the 
connection  is  torsion-free  and  the  second  equation  is  due  to  the  compatibility  of  the  connection  with 
the  metric.  In  section  3,  we  use  the  above  proposition  to  compute  the  Levi-Civita  connection  for 
a  rotating  rigid  body.  The  left  hand  side  of  the  equation  (7)  can  be  calculated  using  the  following 
theorem: 

Theorem  2.2  [6]  Let  a  be  a  one-form  on  a  smooth  manifold  M.  Let  q  E  M  and  Xq,Yq  G  TqM. 
Extend  these  vectors  in  any  smooth  way  to  be  vector  fields  near  q.  Then 

da(Xq,  Yq)  =  Xq(a(Yq))  -  Yq(a(Xq))  -  a([X ,  y]g)  (9) 


Let  X  and  Y  be  two  vector  fields  on  M.  Then  the  vector  fields 


R{X,  Y)Ej:^x^YEj  -  Vy  -  V[X,Y] Ejy  j  =  1,  •  •  •  ,n  (10) 

define  the  curvature  transformation  on  M  for  the  pair  X ,  Y  (please  note  that  our  definition  follows 
those  of  Petersen  [7],  Helgason  [11],  Boothby  [8],  Klingenberg  [10]  and  Frankel  [6]  but  is  the  negative 
of  Do  Carmo’s  definition  [9]).  The  linear  transformation  R(X,Y)  :  #(M)  — ►  \&(M)  has  the  matrix 

R(X,Y))  =  R)klXkYl. 

Consequently,  R^  define  a  mixed  (1,3)  tensor  called  the  curvature  tensor.  It  is  well  known  that 
the  curvature  tensor  satisfies  the  properties  listed  in  the  following  proposition. 

Proposition  2.2  [7]  Let  X,  Y,  Z,  W  be  vector  fields  on  M.  Then: 

1.  <R(X,Y)Z,W>=  -  <  R(Y,  X)Z,  W  >—  -  <R(X,Y)W,Z>; 

2.  <R(X,Y)Z,W>=<R(Z,W)X,Y> ; 

3.  (Bianchi’s  first  identity)  R(X,Y)Z  +  R(Z,X)Y  +  R(Y,Z)X  =  0;  and  v 
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4 ■  (Bianchi’s  second  identity)  (VR)(Z,X,Y,W)  +  (VR)(X,Y,Z,W)  +  (XR)(Y,Z,X,W)  =  0 
where 

(VR)(Z,X,Y,W):=(VzR)(X,Y)W  =  VZ(R(X,Y)W)  -  R(VZX,Y)W 

-R(X,  VZY)W  -  R{X,  Y)XZW 

is  the  covariant  differentiation  of  the  curvature  tensor. 

The  first  two  identities  in  Proposition  2.2  are  used  in  the  next  section.  Cartan  showed  that  the 
curvature  tensor  can  be  obtained  from  the  connection  matrix: 

Proposition  2.3  [7]  The  equations 

Q.  =  du  +  u  A  to  (11) 

=  du)*j  Aw  j  (12) 

define  a  skew-symmetric  matrix  of  2-forms  that  is  related  to  the  curvature  tensor  via 

R(X,  Y)Ej  =  {X,  Y)  <g>  Ei  (13) 


The  above  proposition  is  used  in  Section  3  to  compute  the  curvature  tensor  for  the  rigid  body 
rotation  problem. 

2.2  Derivation  of  first-order  necessary  conditions 

Let  M  be  a  parallelizable  Riemannian  manifold  as  in  the  last  sub-section.  If  c  :  [0. 1]  — »  M  is  a 
differentiable  curve  on  M,  and  X  :  M  — >  TM  is  a  differentiable  vector  field,  then  the  co-variant 
derivative  of  X  along  c(-)  is  defined  in  the  standard  manner  (see  Boothby  [8]  or  Do  Carmo  [9])  to 
be  £jjj£  =  Vc(t)X{t),  t  e  (0, 1).  Let  {Ei,  ■  ■  ■ ,  En}  be  a  frame  of  vector  fields  and  let  (01,  •  •  • ,  6n}  be 
a  frame  of  co- vector  fields  on  M,  so  that  6%{Ej)  =  6);  1  <  i,j  <  n. 

We  define  a  control  system  on  TM  by  a  second-order  vector  field  F  :TMx  1R”1  — >  TTM  defined 
as  follows.  If  7r :  TM  — >  M  denotes  the  projection  operator,  then  a  second-order  vector  field  is  one 
that  satisfies  dnoF^y)  =  (q,  V),  where  q  G  M,  and  V  6  TqM.  If  u  e  1R”\  then  F  is  locally  defined 
by  ((q,V),u)  h->  ((q,  V ),  (V,  f(q,V,u))  or  in  other  words,  by  the  differential  equations: 

q  =  V,  _ 

V  -  f(q,V,u) 
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at  the  point  ( q ,  V )  G  TiW.  Using  the  Levi-Civita  connection  on  M  (as  outlined  in  the  previous 
subsection),  we  can  write  the  above  system  as  one  on  TM  described  by  the  equations: 

q  =  V  =  ViEi, 

=  f(q,V,u)=fi(q,V,u)Ei.  (14) 

at 

The  conditions  on  the  vector  field  /  and  by  extension  the  function  /  will  be  set  forth  in  our  theorem 
on  necessary  conditions.  Such  a  set  of  equations  is  useful  in  describing  the  equations  of  motion  of 
an  air  vehicle  that  is  subject  to  aerodynamic  forces  and  moments  (as  well  as  gravity)  that  depend 
on  its  orientation  with  respect  to  its  velocity  vector,  its  height  above  sea  level,  current  speed  and 
the  deflections  of  its  control  surfaces. 

Now,  let  qoyQi  €  Af,  V&  G  TqoM  and  V\  G  TqiM.  Consider  the  space  CfejO,  1]  of  twice- 
differentiable  maps  q  :  [to,tf]  — »  M  that  satisfy  Equations  (14),  where  tf  >  to?  q(to)  =  <Zo? 
q(tj)  =  qj,  q(to)  =  V0  and  q(tf)  =  Vf.  Then  along  one  such  map  q(-)  the  control  system  takes 
the  form: 

m  =  v(t),  (15) 

^  =  f(q(t),V(t)Mt)),  (16) 

where  u(-)  G  Cm[to,tf],  the  (m-vector  valued)  space  of  continuous  functions.  Suppose  that  one  is 
required  to  find  a  function  u(-)  such  that  the  above  boundary  conditions  axe  satisfied  by  q(-)  while 
minimizing: 

J(u(-))=  ff  L(q(t),V(t),u(t))dt.  (17) 

Jt0 

We  need  the  following  standard  construction  to  describe  the  notion  of  variations  of  a  curve  (we 
use  the  same  notation  here  as  in  Crouch,  Camarinha  and  Silva  Leite  [2]).  Let  ( t,a )  — ♦  q(t,a),  t  G 
[to,tf]  and  a  G  (— e,  e),  e>  0,  be  a  parametrized  family  of  curves  satisfying 

q(t,  0)  =  q(t) 
q{t0,  a)  =  q0 
q{tf,o)  =  qf 
q(to,  a)  =  Vo 

q(tf)a)  =  Vf  (18) 

For  V(t)  G  Tq{t)M  and  pi(t),p2{t)  e  we  define  the  associated  variations: 

V(t,a)  =  V%a)Ei(q(t,a))  G  Tq[t<a)M 
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Pi{t,a)  =  pu{t,a)el(q{t,a))  G  T*[ta)M 

p2{t,a)  =  P2i{t,a)9l(q(t,a))  G  T*{ta)M  (19) 

Define  the  variational  vector  fields 

W(t)  =  6q(t)  :=%(t,  0)  G  Tq{t)M,  t  G  [0,T],  (20) 

W(0)  =  W(T )  =  0;  and  (21) 

W(i)  :=  0)  €  Tq{t)M ,  i  G  [0, T],  (22) 

The  variations  in  the  input  are  denoted  by  u(t ,  a)  £  Rm  with 

6u(t):=^(t, 0)  G  Bm.  (23) 

OCT 


In  the  following,  any  quantity  that  is  described  with  the  second  variable  a  suppressed,  should  be 
construed  as  having  a  —  0  (so,  q(t)  =  q(t ,  0)). 

Then  we  have  the  following  lemma  proved  by  Noakes,  Heinzinger  and  Paden. 

Lemma  2.2  [5]  =  0  and  =  0  for  all  o  €  (— e,  e). 

Crouch,  Camarinha  and  Silva  Leite  set  the  ordinary  derivatives  f~(to>cr),  equal  to  0  and 

thus  obtain  different  results  from  our  work.  We  need  the  following  simple  lemmas  in  the  proof  of 
the  main  theorem  (Theorem  2.3). 

Lemma  2.3 

Pl(t)(^(t))dt  =-£'  ^( 6q(t))dt  (24) 

Proof  We  have  ^(£)  =  ^p(t)  because  the  Levi-Civita  connection  is  symmetric  and  =  0 

(for  full  details  on  the  construction  that  leads  to  this  conclusion,  please  refer  to  Noakes,  Heinzinger 
and  Paden  [5]).  Since  the  Levi-Civita  connection  is  compatible  with  the  Riemannian  metric,  we 
have: 

ft' ftPi(5q)dt  =  jt<'E~1p1,5q>  dt 

=  {<jV-lpiM>  +  <V-1p,,jt5q>)dt 
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that  leads  to  the  “integration  by  parts”  formula: 


Pl(^q)dt=pi(5q)\t/0-  Sq)dt . 

Therefore: 

=  rn(W)ii- 


on  integration  by  parts.  Now  the  result  follows  by  noting  that  W (to)  =  W(t/)  =  0. 


Lemma  2.4 

£'  »«§Tr* = £'  (<eb<2-1^  )  * 


(25) 


Proof  First  we  note  that: 

jC  ' i(  -  f  F>F  +  S  IT* 

by  the  definition  of  the  curvature  tensor  in  Equation  (10),  and  the  fact  that  [(£,  =  0.  By  the 

definition  of  the  linear  isomorphism  E  given  in  Equation  (1)  and  Proposition  2.2,  we  have: 


p2(R(W,V)V)  = 


<X~1P2,R(W,V)V> 

<R{V,'Z~1P2)W,V> 

<R{E~1P2,V)V,W> 

(Eil(E-1p2,V)V)(W) 


Therefore, 

r  p2{t)^wdt = c  ((s^(s_i^.y)^(^)  -  *+»(«ofi. 

on  integrating  the  second  term  by  parts.  Now  Lemma  2.2  proves  the  claim.  I 

We  need  some  more  notation  in  order  to  express  the  necessary  conditions  in  a  compact  form.  As 
{Ei,  -  ■  ■  ,En}  forms  a  global  frame  of  vector  fields  on  M,  the  Jacobi-Lie  brackets  of  the  co-ordinate 
vector  fields  [Ei,  Ej }  can  be  expressed  as  a  linear  combination  of  {Ei,  •  •  • ,  En}: 

[EktEj]  :=  CkjEi-  (26) 
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The  coefficients  Cjy  are  called  structure  constants. 

For  p  E  T*M,  denote: 

Mf)]*P  ■■=  Pi u)  UW  =  Pi4j  fkoj-  (27) 

The  motivation  for  this  definition  is  the  fact  that,  if  F  :  M  ->  N  is  a  Frechet  differentiable 

mapping  between  manifolds  M  and  N,  then  its  differential  DF  maps  TM  to  TN  while  the  pull¬ 
back  ( DF )*  maps  T*N  to  T*M.  They  are  related  as  follows:  For  q  E  M,  suppose  fj  =  fjiO1  E  Tp^N, 
rj  =  E  T*M  are  one-forms  related  by  p  —  ( DqF)*(fj )  (where,  {01,  •  •  • ,  9n}  and  (01,  •  •  • ,  9rn }  form 

a  basis  of  one-forms  in  local  co-ordinates  for  T*M  and  Tp^N  respectively).  If  v  =  iPEj  E  TqM, 

then  p(v)  =  iyvj  =  fn{DqF)}\P  for  all  v  6  TqM  so  that  r?  =  ( DqF)*(fj )  =  f}i{DqF))6:i . 

Similarly  denote: 

[C(f)]*p  :=  PiC)  (f)V 

=  PiCij  fk0j.  (28) 

The  following  theorem  is  the  main  result  of  this  paper.  It  establishes  the  first-order  necessary 
conditions  for  the  curve  (qo,  Vo,  ^o)(t),  t  €  to  be  optimal. 

Theorem  2.3  Suppose  that  (qo,Vo,uo)(t),  t  e  minimizes  the  cost  function  (17),  while  satis¬ 

fying  Equations  (15,  16)  and  boundary  conditions  q(to)  =  qo,  q{tf )  =  q /,  q(to)  =  Vo  and  q(tf)  =  Vf. 
Further  suppose  that 

•  /  and  L  are  differentiable  functions  of  their  arguments , 

•  the  linearized  system  is  controllable  at  the  origin. 

Then  there  exists  one-forms  p\{t),p2{t),  t  G  [0 ,T]  and  differentiable  a.e.  on  [0 ,T]  such  that: 

=  Lq(q0,Vo,uQ)  +  ZR(Z-WV0)Vo-(f:(q0,Vo,u0)  +  u*(f)-C*(f))p2 
Epi  =  —  pi  -f  Ly(qo,  Vo,  wo)  —  fy  (qo,  Vo,  ^o)P2 

where  f  denotes  the  vector  field  f(qo ,  Vq,  uq ), 

•  fu(qoyVo,uo)p2  =  Lu(qo,V0,uo) 

•  the  Hamiltonian  function 

H(qoyVo,uo,p1,p2)(t)  =  L(qo,V0,u0){t)  -pi(V0){t)  -p2(f(qo>  Vo,u0))(t) 
is  a  constant  for  t  £  [to,tf]. 


12 


The  proof  of  the  above  theorem  will  be  given  shortly.  First  we  make  some  important  remarks.  The 
theorem  that  yields  the  existence  of  the  one-forms  Pi(-),P2(')  is  the  Lagrange  Multiplier  theorem: 

Theorem  2.4  [12]  Let  f  :  X  ->  R  and  H  :  X  — >  Z  be  continuously  Frechet  differentiable,  where 
X,Z  are  Banach  spaces.  Suppose  that  f  has  a  local  extremum  under  the  constraint  H(x)  =  0  at 
the  regular  point  xq.  Then,  there  exists  an  element  Zq  G  Z*  (Z*  is  the  dual  space  of  Z),  such  that 
the  functional  f(x)  +  ZqH(x )  is  stationary  at  xo ,  ie.,  f'(xo)  +  ZqH'(x o)  =  0. 

The  term  regular  point  in  the  above  theorem  is  defined  below: 

Definition  2.2  Let  T  be  a  continuously  Frechet  differentiable  map  from  an  open  set  D  in  a  Banach 
space  X  into  a  Banach  space  Y.  If  x0  G  D  is  such  that  T’(x 0)  maps  X  onto  Y,  the  point  xo  is  said 
to  be  a  regular  point  of  the  transformation  T. 


Typically,  theorems  showing  first  order  necessary  conditions  are  proved  as  follows  [12].  Let  the 
system  be  given  by: 

x{t)  =  f(x,  u ),  x(tQ),  x(tf )  fixed, 


and  the  functional  to  be  minimized  be 


J(x(-),u(-))  =  /  l(x,u)dt, 
Jtn 


where  x  G  IR"  and  u  G  ]Rm;  that  is,  we  are  working  in  some  local  co-ordinates.  The  differential 
equation  is  equivalent  to  the  integral  equation: 


:(t)-x(t0)~  f  /(x(r),u(r))dr  =  0; 
Jtn 


that  is, 


A(x,  u)  =  0, 


where  A  :  X  x  U  ->  X.  If  X  =  Cn[t0,tf]  and  U  =  Cm[t0,tf],  then,  the  Frechet  differential  of  A 
exists;  is  continuous  under  our  assumptions;  and  is  given  by: 

SA(x,  u\  h,  v )  =  h(t)  -  [  fxh{r)dT  - 
Jto 

for  (h,  v)  G  X  x  U.  Luenberger  [12]  shows  that  if  the  linearized  system  is  controllable  at  the  origin 
-  specifically,  there  exists  a  continuous  function  v  such  that  the  equation: 

h(t)=  [  fx(x,u)h(r)dT  +  [  fu{x,u)v{r)dT 
Jto  Jto 
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has  a  solution  with  h(ti)  =  e  for  any  e  G  IR”,  then,  (x,u)  is  a  regular  point  for  the  constraint 
A(x,u )  =  0. 

In  this  case,  by  the  Lagrange  multiplier  theorem,  there  exists  a  A(-)  G  NBVn[to,tf]  (the  space  of 
regular  functions  of  bounded  variation)  such  that 


J(x0,  u0,  A)  =  f  f  l(x o,  uo)dt  +  f  1  d\'(t)  x0(t)  -  x0(0)  -  f  f(x0,  u0)dr 

Jto  Jto  L  Jto 


is  stationary  at  the  optimal  solution  (xo,uq).  On  integration  by  parts,  we  get: 

rtf 

J (xo,  u0,  A)  —  I  [1(xq ,  Wo)  4"  A  (t) (/* (cc,  'll)  x)j  dt  ~b 

Jto 


A !(t)  \  xo(t)  -  x0(t0)  -  J*  f(x o>  wq)  dr 


As  xo(tf)  -  xo(to)  +  f(xo,uo)dt  =  0  we  are  led  to  the  conclusion  that  the  augmented  cost 
function 

rtf 

J(xo,uo,  A)  =  /  [l(xo,uQ)  +  \'(t){f(x,u)-x)]dt 
Jto 

is  stationary  at  the  point  (x0,  u0, A)  G  Cn[t0,tf}  x  Cm[tQ,tf}  x  NBVn[t0,tf].  Therefore,  in  the  proof 
of  theorem  2.3,  we  focus  attention  on  the  augmented  cost  function  directly. 


(Proof  of  Theorem  2.3) 

For  each  a  €  (— e,  e),  consider  the  augmented  cost  function: 


=  J  (L{q,V,u)+p1(q-V)+p2  -  f(q,V,u )))  dt, 

't0  K  '  (29) 

where  we  have  denoted  q(t,a),V(t,a),pi(t,a)  and  P2{t,cr)  in  the  integral,  by  q,V,p\,p2  for  con¬ 
ciseness. 

Before  proceeding  further,  we  note  that  by  the  Chain  Rule: 


J^L(q(t,  0),  V (£,  0),  w(t,  0)) 


Similarly, 


Lq{q{t,  a),  V (t, a),u(t,  a)) §* 

+LVi{q{t,  a),  V'it,  a)Ei(t,  a  ),u(t,  o))Jfe  (V*(f,  cr)Ei{t,  a)) 
+Lui(q{t,a),V{t,a),u(t,a))^\ 

J  (7—1) 

Lq{q,  V,  u)(t, 0 )(5q(t))  +  Lv(q,  V,  u)(t,  0)  (^(<)) 

+Lu{q,  V,  u)(t,  0) ( 8u(t )). 

(30) 


d_ 

da 


f(q,V,u)(t,  0) 


(q,V,u)(t,0)Ek(t,  0) 
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-  [fq(q,V,u)(t,0)(5q)  +  f^(q,V,u)(t,0)(— (t))  ••• 

+/£(«,  V, u){t,  0)(^(t)))  £*(*, 0)  +  /*(<?,  V, «)(*,  0) 


DEk(t,  0) 


Lets  consider  the  last  term  in  the  above  equation. 


fk(q,V,u)(t,  0)^f-  =  fk(q,V,u)(t)VSq{t)Ek(t) 

=  /fc(<7,  V,u)(t)ujjkSq^  Ei 

=  f^q^um^-^SqfEi.  (32) 

Now  by  Lebesgue’s  Dominated  Convergence  Theorem,  we  have: 

^(9(-.0),y(-,0),pi(-,0),p2(-,0))  =  Jif-^(L{q,V,u)+p1(q-V)+p2(^-f{q,V,u))j  dt 

=  i:  (^(9’y’u)+pi(S)~piw+p2(^^r) 

-P2T^f(q,V,u)j  dt. 

By  Lemmas  2.3,  2.4  and  Equations  (30  -  32),  we  get: 

-¥-(q(-,0),V(-,0),Pi(-,0),P2(-,0))  =  /  (Lg(g,y,u)5g  +  Lv(g,y,u)5F  +  L„(g,F,u)5u 

w  ‘/to 

~^(Sq)  -Pl(SV)  +  VR{Y,-'p2,V)V{8q)  -  ^(«V) 
-P2(fq(Sq))~P2{fvSV)-p2(fu8u) 

-Hf))*P2(5q)  +  [C(f)]*p2(Sq))dt 

As  the  variations  <5g,  SV  and  5u  are  arbitrary,  subject  to  Sq(to)  —  8q(tf)  =  SV(to)  =  8V(tj)  —  0 
we  have  the  first  two  statements  of  the  theorem. 

To  prove  the  last  statement  of  the  theorem,  consider 

H(t)  =  |L(9o)Vb,uo)(t)-^(^o)-Pi(^)W-^(/(9o,Vb,«o))W 

~P2(^f{qo,V0,uo))(t) 

DV 

=  Lq(qo,  Vb,«o)(V&)  +  Lv{qo ,  Vo,uo)(-^-)  +  Lu(qo ,  Vo,uq)(u) 

-  {Lq(q0,  V0,  u0)  +  EiJ(S-1p2,  Vo)V0  -  (/,*(«>,  Vo,  «o)  +  w*(/)  -  <?*(/))p2)  (V0)(t) 
-Pi{f(qo,Vo,u0))(t)  -  (-pi  +  Lv{qo,Vo,u0)  -  fv{qo,V0,u0)p2)  (f(qo,Vo,u0))(t) 

~P2  (fq{qo,Vo,uo)(Vo)  +  fv{qo,Vo,uo)(-jj-)  +  fu(qo>Vb,uo)(u)  +  y 
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by  Equations  (15), (16);  the  first  two  statements  that  we  just  proved;  and  the  fact  that 


<R('E-lP2,Vo)Vo,V0>=0 


by  Proposition  2.2. 


3  Applications 

In  this  section,  we  specialize  the  results  of  the  last  section  to  obtain  a  formula  for  cubic  splines 
on  Riemannian  manifolds  and  to  rotational  rigid  body  dynamics.  We  then  present  a  numerical 
example  where  we  apply  a  new  numerical  method  called  the  Modified  Simple  Shooting  Method[  13] 
and  solve  the  resulting  two-point  boundary  value  problem  for  the  rotational  rigid  body  dynamics 
problem. 


3.1  Cubic  splines  on  Riemannian  manifolds 


Here  we  specialize  Theorem  2.3  and  recover  the  Noakes,  Heinzinger  and  Paden  formula  for  cubic 
splines  on  Riemannian  manifolds  [5].  Let  M  be  a  parallelizable  Riemannian  manifold  and  let 
q0,qi  e  M,  V0  €  TqoM  and  V\  €  TqiM.  Consider  the  problem: 

rtf 

Minimize  J(u(-))  =  /  u2{t)dt 

Jto 


subject  to: 


m  =  V(t),  (33) 

^  =  u(t)  =  U<(i)Ei(t),  (34) 


and 


q(t0 )  =  qo 

q(t/)  =  qf 

q{to )  =  Vb 

q(tf)  =  vf 

(35) 

Thus  we  have  f(q,  V,u)  =  u  and  £  is  the  identity  matrix.  Therefore,  there  is  an  identification  of 
vectors  and  co-vectors.  Then,  Theorem  2.3  asserts  the  existence  of  one-form  sections  p{(t),p2(t) 
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such  that: 


Dpi 

dt 

PP2 

dt 

P2 


R(p2,V)V-(u*(f)-C*(f))p2 

- Pi 

u, 


where  we  have  used  the  identification  of  vectors  and  co-vectors  in  the  last  equation.  Thus 
D2V  Du  Dp2 


dt 2 
D3V 
~dD~ 


dt 

Dpi 

dt 


dt 


=  -Pi 


=  -R(p2,V)V  +  (u>*(f)-C*(f))p2. 


Let  w  G  ^(M).  Then 


(C 0*(f)-C*(f))p2(w) 


P2i(vlj  (P2)  ~  C)  {P2))wj 
PZiUjkPW 
{w)p\p2l 

0  for  all  w  G  \I/(M) 


because  u)\  —  by  Proposition  2.1.  Therefore, 


DZV 

dt 


DV 

+  R(—,V)V  =  0, 


(36) 


which  is  the  equation  for  a  cubic  spline  that  was  first  obtained  by  Noakes,  Heinzinger  and  Paden 
[5]. 


3.2  Rigid  body  rotation 


Consider  a  rigid  body  with  principal  moment  of  inertia  matrix  I  and  mass  m.  The  configuration 
space  of  a  rigid  body  SE(3)  is  the  space  of  rotations  given  by  the  set  of  3  x  3  matrices  50(3)  = 
=  I;  det(i?)  =  1.  R  is  the  orientation  of  the  rigid  body  with  respect  to  an  earth-fixed 
co-ordinate  system. 

The  angular  velocity  of  the  body  in  the  principal  axis  system  (called  the  body  axis  system)  centered 
at  the  center  of  mass  is  defined  as:  ft  =  Q'Q  (where  '  denotes  the  matrix  transpose).  If  we  define 
the  skew-symmetrization  of  D!  =  [£lj ,  0,2,  H3]  to  be 


Cl  = 


0  —  CI3 

H3  0 


— &2  fll 
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£^2 
— £2i 
0 


then  the  Euler’s  equations  for  a  rigid  body  can  be  written  in  a  compact  form  as: 


(37) 

(38) 


Q  =  QCl 

Cl  -  Cl  x  I-1f2  =  I -1Te. 

where  Te  are  the  moments  acting  on  the  body  expressed  in  the  body  axis  system.  In  the  following, 
we  treat  I _1Te  as  the  input  vector  u. 

We  now  derive  the  same  equations  using  a  geometrical  method.  Let  {ei,e2,e3}  form  a  basis  for 
the  Lie  algebra  so(3)  of  the  Lie  Group  50(3).  Then  one  can  form  a  parallel  frame  at  each  point 
Q  e  50(3)  given  by  Ei(Q)  =  Qei,  i  -  1,2,3  for  the  Lie  group  SE( 3).  For  this  parallel  frame 
{E\,E2,E$}  let  the  structure  constants  for  the  Jacobi-Lie  bracket  can  be  obtained  from  those 
for  the  Lie  algebra  bracket  via  the  following  computation: 

[Ei{Q),Ej(Q)\j-L  =  [QeuQej] 

—  Q[&ii 

where  [•,  -\j~l  denotes  the  Jacobi-Lie  bracket  of  vector  fields  and  [•,  -}la  denotes  the  Lie  algebra 
bracket  of  vectors  in  so(3).  Suppose  we  chose  the  structure  constants  for  the  Lie  algebra  bracket  to 
be  as  in  the  following  table. 


c?2  =  i 

C23  =  ~T1 

1 

II 

'IS5 

c-  =  0  for  all  other  1  <i,j,k<  3. 

Then  the  structure  constants  for  the  Jacobi-Lie  bracket  of  co-ordinate  frame  vector  fields  Cljk  is 
given  by: 

1<W,*<3. 

The  Riemannian  metric  is  defined  via  the  following  table: 


<E\,  E\  >=  |/i 

<E2,E2>=  \l2 

<£?3,£?3>=  \l% 

<Ei,Ej>—  0  for  all  other  1  <  i,j  <  6. 

We  can  compute  the  connection  matrix  for  the  rotating  rigid  body  using  Proposition  2.1  and 
Theorem  2.2.  If  {fl1,#2,#3}  is  a  set  of  co-vector  fields  on  50(3)  such  that  Oi(Ej)  =  then, 
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Equation  (9)  leads  to: 


de\Eh Ek)  =  Ejie^Ek))  -  Ek{e\Ej))  -  e\[Eh Ek}) 

=  -P([Ej,Ek]). 

We  now  equate  the  left-hand-side  of  the  above  equation  with  that  of  Equation  (7)  and  compute 
(by  solving  for  the  one-forms  io\ ’s)  the  following  connection  matrix: 


o  ao 3  pe2 

-ad3  0  7  01 

-I362  -701  0 

where  a  =  (-£  -  £  +  ,  P  =  (£  -  £  +  &)  and  7  = 

the  matrix  is  skew-symmetric. 

Next,  we  compute  the  matrix  VeE  using  Equation  (3): 


(39) 


-$(£"&“£)•  Note  that 


[VEjEi\  =  [ioki  (Ej)Ek  = 


3 

0 

-PE3 

—aE2 

-jE3 

0 

aE\ 

iE2 

fiE\ 

0 

(40) 


Using  the  above  matrix,  we  can  obtain  an  expression  for  the  co-variant  derivative  of  a  vector 
field  on  50(3)  with  respect  to  another  vector  field  on  50(3).  Let  17,  £  £  \I>(50(3))  and  q(t)  be  the 
curve  obtained  by  solving  the  system  q(t)  =  17;  #(0)  =  qo]  t  G  [0, 1].  Then  along  the  curve  g(-),  we 
compute: 


where 


a£2173  +  P(,3fl2 

Vn£  = 

+ 

— a£Jft3  + 

i 

CO 

-pt'Q?  - 

0  = 


7  0  0 
0-/3  0 
0  0a 


(41) 


(42) 
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In  particular, 


(43) 


—  =  Vfift  =  fi  +  n  x  ©a 

at 

It  is  interesting  to  note  that  even  though  ©  7=  I-1,  we  still  have  II  x  OH  —  II  x  I-1  II  ! 

We  compute  the  curvature  tensor  tf(£,H)H  using  Proposition  2.3.  The  curvature  two-form 
matrix  turns  out  to  be  (we  apologize  for  the  the  use  of  H  to  denote  both  angular  velocities  and  the 
curvature  two-form,  but  the  curvature  two-form  only  appears  on  this  page): 

0  (§  +  P1W1  a  e2  (£  +  c*7)03  a  o1 

+  A02  0  (l  +  afle2  A93 

_(|  +  a7)^3A0i  _(7.  +  a(3)92  A  93  0 

As 

R(Z,n)Ej  =  nij 

we  have: 

i?(£,  11)11  =  II  x  Ai(fxII),  (45) 

where 

’  £  +  a/3  0  0 

Ai  =  0  ~{h+  a7)  0 

0  0  £  +  07 
Finally,  we  compute  the  (w*(/)  -  C*(f))p2  term  that  appears  in  Theorem  2.3: 

(«*(/)  -  C*(f))'P2  =  P2,  K-  («fc£fcK  -  C+  (ufcEfc)^);  1  <  t,  j,  k  <  3. 

After  extensive  computations,  the  right  hand  side  turns  out  to  be: 

Ul  P2X 

(u>*(f)-C*(f))p2  =  Q  U2  x  pi2  .  (46) 

u3  _  .  P23  _ 

Theorem  2.3  postulates  the  existence  of  one- forms  pi(t)  and  P2(t)  for  t  S  [to,t/]-  Define  the  vector 
fields  £(•)  and  ??(•)  along  q(-),  via  the  identification  £(t)  —  E-1pi(f)  and  77(f)  =  S_1p2(f)-  Then,  we 
can  write  the  necessary  conditions  in  terms  of  ( Q ,  H,  £,  77).  The  full  set  of  equations  are  then: 

Q  =  QCl 

ii  —  -n  x  0H  +  77 
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(47) 

(48) 


(49) 

(50) 


£  =  -£  x  +  fi  x  Ai(77  x  Q)  —  ©S  1tj  x  77 
77  =  —  77  x  0f2  —  £, 

with  (<5,fi)(io)  and  (Q,Cl)(tf)  specified. 

4  Numerical  Experiments 

The  equations  (47-50)  constitute  a  two-point  boundary  value  problem.  We  used  a  modified  shooting 
method  technique  [13]  to  numerically  solve  for  the  unknown  ’’Lagrange  multipliers”  (£,77)  at  initial 
time. 

The  first  equation  (47)  is  a  matrix  equation  that  we  integrated  using  the  well-known  Rodriguez’s 
formula  [141: 

/  o  Cl 2  \ 

Q(t  +  h)  =  Q(t)  (  /  +  p^sinfllfiHh)  +  pp  (1  -  cos(||Q||h))  J  ,  (51) 

where  h  is  the  time-step  for  integration.  This  results  in  a  Q  matrix  that  “stays”  on  the  group 
50(3)  at  each  time-step. 

The  moments  of  inertia  constants  for  the  numerical  simulation  were  chosen  arbitrarily  to  be: 
/1  =  10;  h  =  5;  73  =  2.5.  The  initial  time  t0  was  set  to  0  and  the  final  time  tf  was  chosen  to  be  1. 
The  initial  states  (Q,  ft)(0)  and  the  desired  final  states  {Qdes,  tides)  at  t  =  1,  were  chosen  using  the 
pseudo-random  number  generating  program  in  MATLAB.  Their  values  for  a  simulation  run  are 
listed  in  the  following  table  (to  3  significant  digits). 


Q(  0) 

P(0) 

0.572 

0.817 

0.079 

0.950 

-0.783 

0.572 

-0.246 

4.337 

-0.246 

0.079 

0.966 

7.0923 

Qdes(l) 

tides(l) 

-0.268 

0.963 

-0.011 

1.901 

-0.838 

-0.227 

0.497 

8.673 

0.476 

0.142 

0.868 

4.185 

The  initial  value  for  the  co-states  (£,77)(0)  was  also  chosen  using  the  pseudo-random  number  gen¬ 
erating  program  in  MATLAB  and  it  turned  out  to  be 
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£(0) 

v{  o) 

0.116 

0.034 

0.078 

0.192 

0.369 

0.471 

The  modified  simple  shooting  method  involves  the  choice  of  a  continuous,  time-parametrized 
reference  path  that  connects  the  initial  and  final  points.  For  t  €  [0, 1],  we  picked  the  reference  path 
to  be: 

Qrefit)  =  Q(  0)  exp(0f),  where  $  =  ln(Q(0)-1<3des) 

nref{t)  =  fi(o)  +  (ndes  -  fi(o))t. 

The  equations  were  integrated  in  the  forward  direction  until  at  some  time  t  €  (0, 1],  we  had 

\\q(t)  -  qref(t)\\  =  100  *  ||Q(t)  -  Qref(t)\\M  +  !!«(*)  ~  Are/WII  >  200.  (52) 

At  this  point,  the  initial  guesses  for  the  Lagrange  multipliers  (£,  r])( 0)  were  updated  via  the  modified 
Newton’s  method  until  \\q(t)  -  qref(t) ||  <  0.5.  Then  the  equations  are  integrated  forward  again 
until  the  inequality  in  (52)  is  satisfied.  The  vector  (£,7?)(0)  is  updated  as  before  and  the  iteration 
is  repeated.  The  orientation  matrix  norm  was  calculated  as:  ||Q||m  —  ||lnQl  where  ||  •  ||  is  the 
standard  matrix  norm.  The  norm  on  the  orientation  matrix  was  multiplied  by  100  so  that  the 
final  orientation  is  met  accurately.  The  time  step  for  the  integration  was  0.02  seconds.  The  CPU 
time  taken  for  the  computations  in  a  MATLAB  environment,  running  on  a  1.8  GHz  PC  was  32.94 
seconds. 

The  solution  of  the  two-point  boundary  value  problem  led  to  the  following  final  states  at  t  =  1: 


Q(  i) 

ft(l) 

-0.268 

0.963 

-0.011 

1.901 

-0.838 

-0.227 

0.497 

8.673 

0.476 

0.142 

0.868 

4.185 

The  program  converged  to  the  following  value  of  the  co-states  at  t  =  0. 


m 

7?(0) 

-56.132 

17.794 

-84.811 

47.803 

-65.834 

33.502 
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Figure  2:  Plot  of  ZYX  Euler  angles. 


Figures  2-4  show  the  results  of  the  simulation.  The  ZYX  Euler  angles  in  Figure  2  was  computed 
according  to: 


P  -  -sin  ^Qsi) 


Though  7 (t)  in  the  figure  seems  to  be  not  continuously  differentiable  at  a  point,  one  can  clearly 
see  from  the  plots  of  Q3i(t),Q32(t)  and  Quit)  in  Figure  3  that  they  are  indeed  continously  differ- 
entiable. 

5  Conclusion 

In  this  paper,  we  have  made  three  new  contributions.  Firstly,  we  have  derived  first  order  neces¬ 
sary  conditions  for  an  optimal  control  problem  on  a  parallelizable  Riemannian  manifold.  These 
equations  specialize  to  those  of  cubic  splines  on  Riemannian  manifolds  that  were  first  discovered  by 
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seconds 


Figure  4:  Plot  of  angular  velocities, 


Noakes,  Heinzinger  and  Paden.  Secondly,  we  have  specialized  the  equations  to  a  rigid  body  rotation 
problem.  Thirdly,  we  have  presented  the  results  of  numerical  experiments  where  we  solve  the  two 
point  boundary  value  problem  resulting  from  the  necessary  conditions.  We  used  a  modified  simple 
shooting  method  for  the  numerical  solution  of  the  problem  and  the  results  indicate  the  feasibility 
of  online  implementation  of  the  path  planning  problems. 
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